import numpy as np

def angle(i):
    return np.arctan(1/(2**i))

def angle_h(i):
    return np.arctanh(1/(2**i))

def k(n):
    k = 1
    for i in range(n):
        k = k / np.sqrt(1 + 2**(-2*i))

    return k

def kh(n):
    k = 1
    for i in range(1,n+1):
        k = k / np.sqrt(1 - 2**(-2*i))
    return k

def cordic(theta, n):
    #90 > theta > 0
    x = 1
    y = 0
    z = theta
    for i in range(n):
        if z > 0:
            d = 1
        else:
            d = -1
        x_tmp = x - d * y / (2**i)
        y_tmp = y + d * x / (2**i)
        x = x_tmp
        y = y_tmp
        z = z-d*angle(i)

    #print(k(n))
    return x*k(n), y*k(n), y/x

def cordic_h(theta, n):
    #90 > theta > 0
    x = 1
    y = 0
    z = theta
    repeat = 4
    for i in range(1,n+1):
        if z >= 0:
            d = 1
        else:
            d = -1
        x_tmp = x + d * y / (2**i)
        y_tmp = y + d * x / (2**i)
        x = x_tmp
        y = y_tmp
        z = z - d * angle_h(i)
        if i == repeat:
            if z >= 0:
                d = 1
            else:
                d = -1
            x_tmp = x + d * y / (2**i)
            y_tmp = y + d * x / (2**i)
            x = x_tmp
            y = y_tmp
            z = z - d * angle_h(i)
            repeat = repeat * 3 + 1

    #print(k(n))
    return x * kh(n), y * kh(n), (x+y) * kh(n)

def test():
    for i in range(10):
        theta = np.pi/2*np.random.rand(1)[0]
        print("*"*80)
        print(i, theta)
        d_cos, d_sin, d_tan = cordic(theta, 32)
        d_cos_ref, d_sin_ref, d_tan_ref = np.cos(theta), np.sin(theta), np.tan(theta)
        print(d_cos,d_sin,d_tan)
        print(d_cos_ref,d_sin_ref,d_tan_ref)

def testh():
    for i in range(10):
        theta = np.pi/3*np.random.rand(1)[0]
        print("*"*80)
        print(i,theta)
        d_cosh, d_sinh, d_exp = cordic_h(theta=theta, n = 121)
        d_cosh_ref, d_sinh_ref, d_exp_ref = np.cosh(theta), np.sinh(theta), np.exp(theta)
        print(d_cosh,d_sinh,d_exp)
        print(d_cosh_ref,d_sinh_ref,d_exp_ref)


if __name__=='__main__':
    testh()
    #for i in range(16):
    #    print(i,np.round(angle(i)/np.pi*180*(2**16)))
    #print("K={}".format(k(16)*(2**16)))